#include "simulator/simulator_lidar.hpp"


namespace simulator {

    SimulatorLidar::SimulatorLidar() : rclcpp::Node("simulator_lidar") {
        this->declare_parameter("min_angle", -M_PI);
        this->declare_parameter("max_angle", M_PI);
        this->declare_parameter("min_distance", 0.15);
        this->declare_parameter("max_distance", 6.00);
        this->declare_parameter("noise", 0.00);
        this->declare_parameter("size", 400);
        this->declare_parameter("rate", 10);
        this->declare_parameter("use_topic_odom", false);
        this->declare_parameter("odom_topic", "/odom");
        this->declare_parameter("stage_map_topic", "map");
        this->declare_parameter("global_frame", "real_map");
        this->declare_parameter("lidar_frame", "laser_link");

        this->get_parameter("min_angle", min_angle_);
        this->get_parameter("max_angle", max_angle_);
        this->get_parameter("min_distance", min_dis_);
        this->get_parameter("max_distance", max_dis_);
        this->get_parameter("noise", noise_);
        this->get_parameter("size", point_size_);
        this->get_parameter("rate", rate_);
        this->get_parameter("use_topic_odom", use_topic_odom_);
        this->get_parameter("odom_topic", odom_topic_);
        this->get_parameter("stage_map_topic", stage_map_topic_);
        this->get_parameter("global_frame", global_frame_);
        this->get_parameter("lidar_frame", lidar_frame_);

        step_ = (max_angle_ - min_angle_) / (point_size_ - 1);
        tf_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
        lidar_pub_ = this->create_publisher<sensor_msgs::msg::LaserScan>("scan", 1);
        map_sub_ = this->create_subscription<nav_msgs::msg::OccupancyGrid>(
            stage_map_topic_, 1, std::bind(&SimulatorLidar::mapReceived, this, std::placeholders::_1));
        if (use_topic_odom_) {
            odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
                odom_topic_, 10, std::bind(&SimulatorLidar::odomCallback, this, std::placeholders::_1));
        }
        pub_laser_timer_ = this->create_wall_timer(std::chrono::milliseconds(1000 / rate_), std::bind(&SimulatorLidar::pubLaserCallback, this));
    }

    SimulatorLidar::~SimulatorLidar() {}

    double SimulatorLidar::gaussRand(double mu, double sigma) {
        static double V1, V2, S;
        static int phase = 0;
        double X;

        if (phase == 0) {
            do {
                double U1 = (double)rand() / RAND_MAX;
                double U2 = (double)rand() / RAND_MAX;

                V1 = 2 * U1 - 1;
                V2 = 2 * U2 - 1;
                S = V1 * V1 + V2 * V2;
            } while (S >= 1 || S == 0);

            X = V1 * sqrt(-2 * log(S) / S);
        } else {
            X = V2 * sqrt(-2 * log(S) / S);
        }

        phase = 1 - phase;
        return (X * sigma + mu);
    }

    void SimulatorLidar::initLaserScan(sensor_msgs::msg::LaserScan& laser_scan) {
        laser_scan.header.frame_id = lidar_frame_;
        laser_scan.angle_min = min_angle_;
        laser_scan.angle_max = max_angle_;
        laser_scan.angle_increment = step_;
        laser_scan.time_increment = 1 / point_size_ / rate_;
        laser_scan.range_min = min_dis_;
        laser_scan.range_max = max_dis_;
    }

    double SimulatorLidar::normalAngle(double theta) {
        if (theta > M_PI) {
            theta = normalAngle(theta - 2 * M_PI);
        } else if (theta <= -M_PI) {
            theta = normalAngle(theta + 2 * M_PI);
        }
        return theta;
    }

    void SimulatorLidar::getPose(geometry_msgs::msg::TransformStamped& transform, double& start_angle, double& reverse) {
        double roll, pitch, yaw;

        // 监听当前时刻源坐标系到目标坐标系的坐标变换
        try {
            transform = tf_->lookupTransform(
                global_frame_, lidar_frame_, tf2::TimePointZero);
        } catch (const tf2::TransformException & ex) {
            // 如果坐标变换获取失败，进入异常报告
            RCLCPP_INFO(
                this->get_logger(), "Could not transform %s to %s: %s",
                global_frame_.c_str(), lidar_frame_.c_str(), ex.what());
            return;
        }
        
        // 四元数转换为欧拉角
        tf2::Quaternion q(
            transform.transform.rotation.x,
            transform.transform.rotation.y,
            transform.transform.rotation.z,
            transform.transform.rotation.w);
        tf2::Matrix3x3 m(q);
        m.getRPY(roll, pitch, yaw);

        if (pow(roll, 2) + pow(pitch, 2) > esp) {
            start_angle = yaw + max_angle_;
            reverse = -1.0;
        } else {
            start_angle = yaw + min_angle_;
            reverse = 1.0;        }
    }

    bool SimulatorLidar::limit_range(int& mx, int& my) {
        bool x_flag = (mx >= (int)local_map_.info.width) || (mx <= 0);
        mx = x_flag ? std::max(std::min(mx, (int)local_map_.info.width), 0) : mx;
        bool y_flag = (my >= (int)local_map_.info.height) || (my <= 0);
        my = y_flag ? std::max(std::min(my, (int)local_map_.info.height), 0) : my;
        return x_flag || y_flag;
    }

    void SimulatorLidar::distance(int start_x, int start_y, double theta, int& end_x, int& end_y) {
        theta = normalAngle(theta);
        double k, e = -0.5;
        int major_step, minor_step;

        if (limit_range(start_x, start_y)) {
            end_x = start_x;
            end_y = start_y;
            return;
        } else if (std::abs(theta) <= (0.25 * M_PI) || std::abs(theta) >= (0.75 * M_PI)) {
            k = std::abs(std::tan(theta));
            major_step = std::abs(theta) <= (0.25 * M_PI) ? 1 : -1;
            minor_step = theta >= 0 ? 1 : -1;

            while (true) {
                start_x += major_step;
                e += k;
                if (e >= 0) {
                    start_y += minor_step;
                    e -= 1;
                }
                if (limit_range(start_x, start_y) || getValue(start_x, start_y) == OBS_SPACE || getValue(start_x, start_y) == UNKNOWN_SPACE) {
                    end_x = start_x;
                    end_y = start_y;
                    return;
                }
            }
        } else {
            k = std::abs(std::tan(0.5 * M_PI - theta));
            major_step = theta > 0 ? 1 : -1;
            minor_step = std::abs(theta) <= (0.5 * M_PI) ? 1 : -1;

            while (true) {
                start_y += major_step;
                e += k;
                if (e >= 0) {
                    start_x += minor_step;
                    e -= 1;
                }
                if (limit_range(start_x, start_y) || getValue(start_x, start_y) == OBS_SPACE || getValue(start_x, start_y) == UNKNOWN_SPACE) {
                    end_x = start_x;
                    end_y = start_y;
                    return;
                }
            }
        }
    }

    void SimulatorLidar::mapToWorld(int mx, int my, float& wx, float& wy) {
        wx = local_map_.info.origin.position.x + (mx + 0.5) * local_map_.info.resolution;
        wy = local_map_.info.origin.position.y + (my + 0.5) * local_map_.info.resolution;
    }

    void SimulatorLidar::worldToMap(float wx, float wy, int& mx, int& my) {
        mx = (int)((wx - local_map_.info.origin.position.x) / local_map_.info.resolution);
        my = (int)((wy - local_map_.info.origin.position.y) / local_map_.info.resolution);
    }

    char SimulatorLidar::getValue(int mx, int my) {
        int index = my * local_map_.info.width + mx;
        if (index < 0 || index >= (int)data_length_) {
            return ERROR;
        } else {
            return local_map_.data[index];
        }
    }

    char SimulatorLidar::getValue(float wx, float wy) {
        int mx, my;
        worldToMap(wx, wy, mx, my);
        return getValue(mx, my);
    }

    void SimulatorLidar::distance(float start_x, float start_y, double theta, float& end_x, float& end_y) {
        int start_mx, start_my, end_mx, end_my;
        worldToMap(start_x, start_y, start_mx, start_my);
        distance(start_mx, start_my, theta, end_mx, end_my);
        mapToWorld(end_mx, end_my, end_x, end_y);
    }

    void SimulatorLidar::getFrame(sensor_msgs::msg::LaserScan& laser_scan) {

        geometry_msgs::msg::TransformStamped transform;
        double start_angle, reverse, angle;
        float start_wx, start_wy, end_wx, end_wy, dis;

        laser_scan.header.stamp = this->get_clock()->now();
        getPose(transform, start_angle, reverse);
        start_wx = (float)transform.transform.translation.x;
        start_wy = (float)transform.transform.translation.y;
        laser_scan.ranges.clear();

        for (int i = 0; i < point_size_; i++) {
            angle = start_angle + i * reverse * step_;
            distance(start_wx, start_wy, angle, end_wx, end_wy);
            dis = sqrt(pow(end_wx - start_wx, 2) + pow(end_wy - start_wy, 2));
            dis += dis * gaussRand(0, noise_);
            if (dis <= max_dis_ && dis >= min_dis_) {
                laser_scan.ranges.push_back(dis);
            } else {
                laser_scan.ranges.push_back(std::numeric_limits<float>::infinity());
            }
        }
    }

    void SimulatorLidar::pubLaserCallback() {
        sensor_msgs::msg::LaserScan laserscan;
        initLaserScan(laserscan);
        getFrame(laserscan);
        lidar_pub_->publish(laserscan);
    }

    void SimulatorLidar::mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr grid_map) {
        local_map_ = *grid_map;
        data_length_ = grid_map->info.width * grid_map->info.height;
    }

    void SimulatorLidar::odomCallback(const nav_msgs::msg::Odometry::SharedPtr odom_msg) {
        odom_msg_ = *odom_msg;
    }
}

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<simulator::SimulatorLidar>());
    rclcpp::shutdown();
    return 0;
}
